#!/usr/bin/env python
#coding=utf-8



#python plot_data.py ../offboard_pkg/log/20180128_235901_fly.log param1 param2 param3


import rospy
import os
import json
import numpy as np
import math
import time
import threading
from std_msgs.msg import Float32MultiArray




def spin():
    rospy.spin() 


def plot_param1_cb(msg):
    global plot_param1_receiver
    plot_param1_receiver = msg.data
    pos_error_x = plot_param1_receiver[0]
    pos_error_y = plot_param1_receiver[1]
    pos_error_z = plot_param1_receiver[2]

    vel_error_x = plot_param1_receiver[3]
    vel_error_y = plot_param1_receiver[4]
    vel_error_z = plot_param1_receiver[5]

    mavPos_x = plot_param1_receiver[6]
    mavPos_y = plot_param1_receiver[7]
    mavPos_z = plot_param1_receiver[8]

    target_pos_x = plot_param1_receiver[9]
    target_pos_y = plot_param1_receiver[10]
    target_pos_z = plot_param1_receiver[11]

    mavVel_x = plot_param1_receiver[12]
    mavVel_y = plot_param1_receiver[13]
    mavVel_z = plot_param1_receiver[14]

    target_vel_x = plot_param1_receiver[15]
    target_vel_y = plot_param1_receiver[16]
    target_vel_z = plot_param1_receiver[17]
    # print("time: {}\npos_error_x: {}".format(rospy.Time.now(), pos_error_x))
    # print("time: {}\npos_error_y: {}".format(rospy.Time.now(), pos_error_y))
    # print("time: {}\npos_error_z: {}".format(rospy.Time.now(), pos_error_z))

    # print("time: {}\nvel_error_x: {}".format(rospy.Time.now(), vel_error_x))
    # print("time: {}\nvel_error_y: {}".format(rospy.Time.now(), vel_error_y))
    # print("time: {}\nvel_error_z: {}".format(rospy.Time.now(), vel_error_z))

    # print("time: {}\nmavPos_x: {}".format(rospy.Time.now(), mavPos_x))
    # print("time: {}\nmavPos_y: {}".format(rospy.Time.now(), mavPos_y))
    # print("time: {}\nmavPos_z: {}".format(rospy.Time.now(), mavPos_z))

    # print("time: {}\ntarget_pos_x: {}".format(rospy.Time.now(), target_pos_x))
    # print("time: {}\ntarget_pos_y: {}".format(rospy.Time.now(), target_pos_y))
    # print("time: {}\ntarget_pos_z: {}".format(rospy.Time.now(), target_pos_z))

    # print("time: {}\nmavVel_x: {}".format(rospy.Time.now(), mavVel_x))
    # print("time: {}\nmavVel_y: {}".format(rospy.Time.now(), mavVel_y))
    # print("time: {}\nmavVel_z: {}".format(rospy.Time.now(), mavVel_z))

    # print("time: {}\ntarget_vel_x: {}".format(rospy.Time.now(), target_vel_x))
    # print("time: {}\ntarget_vel_y: {}".format(rospy.Time.now(), target_vel_y))
    # print("time: {}\ntarget_vel_z: {}".format(rospy.Time.now(), target_vel_z))

def plot_param2_cb(msg):
    global plot_param2_receiver
    plot_param2_receiver = msg.data

    current_point_x = plot_param2_receiver[0]
    current_point_y = plot_param2_receiver[1]
    current_point_z = plot_param2_receiver[2]
    planned_point_x = plot_param2_receiver[3]
    planned_point_y = plot_param2_receiver[4]
    planned_point_z = plot_param2_receiver[5]
    linear_x = plot_param2_receiver[6]
    linear_y = plot_param2_receiver[7]
    linear_z = plot_param2_receiver[8]
    angular_z = plot_param2_receiver[9]

    if first_time == True:
        time_now = rospy.Time.now()
        first_time = False


    # print("time: {}\ncurrent_point_x: {}".format(rospy.Time.now()-first, current_point_x))
    # print("time: {}\ncurrent_point_y: {}".format(rospy.Time.now()-first, current_point_y))
    # print("time: {}\ncurrent_point_z: {}".format(rospy.Time.now()-first, current_point_z))
    # print("time: {}\nplanned_point_x: {}".format(rospy.Time.now()-first, planned_point_x))
    # print("time: {}\nplanned_point_y: {}".format(rospy.Time.now()-first, planned_point_y))
    # print("time: {}\nplanned_point_z: {}".format(rospy.Time.now()-first, planned_point_z))
    # print("time: {}\nlinear_x: {}".format(rospy.Time.now()-first, linear_x))
    # print("time: {}\nlinear_y: {}".format(rospy.Time.now()-first, linear_y))
    # print("time: {}\nlinear_z: {}".format(rospy.Time.now()-first, linear_z))
    # print("time: {}\nangular_z: {}".format(rospy.Time.now()-first, angular_z))



def plot_param3_cb(msg):
    global plot_param3_receiver
    plot_param3_receiver = msg.data
    pos_local_x = plot_param3_receiver[0]
    pos_local_y = plot_param3_receiver[1]
    pos_local_z = plot_param3_receiver[2]

    angle_local_x = plot_param3_receiver[3]
    angle_local_y = plot_param3_receiver[4]
    angle_local_z = plot_param3_receiver[5]

    pos_tag_x = plot_param3_receiver[6]
    pos_tag_y = plot_param3_receiver[7]
    pos_tag_z = plot_param3_receiver[8]

    tag_angle_lv1 = plot_param3_receiver[9]
    tag_angle_lv2 = plot_param3_receiver[10]
    tag_angle_lv3 = plot_param3_receiver[11]

    # if first_time == True:
    #     time_now = rospy.Time.now()
    #     first_time = False

    

    print("time: {}\npos_local_x: {}".format((rospy.Time.now()-time_now)/100000000.0, pos_local_x))
    print("time: {}\npos_local_y: {}".format((rospy.Time.now()-time_now)/100000000.0, pos_local_y))
    print("time: {}\npos_local_z: {}".format((rospy.Time.now()-time_now)/100000000.0, pos_local_z))

    print("time: {}\nangle_local_x: {}".format((rospy.Time.now()-time_now)/100000000.0, angle_local_x))
    print("time: {}\nangle_local_y: {}".format((rospy.Time.now()-time_now)/100000000.0, angle_local_y))
    print("time: {}\nangle_local_z: {}".format((rospy.Time.now()-time_now)/100000000.0, angle_local_z))

    print("time: {}\npos_tag_x: {}".format((rospy.Time.now()-time_now)/100000000.0, pos_tag_x))
    print("time: {}\npos_tag_y: {}".format((rospy.Time.now()-time_now)/100000000.0, pos_tag_y))
    print("time: {}\npos_tag_z: {}".format((rospy.Time.now()-time_now)/100000000.0, pos_tag_z))

    print("time: {}\ntag_angle_lv1: {}".format((rospy.Time.now()-time_now)/100000000.0, tag_angle_lv1))
    print("time: {}\ntag_angle_lv2: {}".format((rospy.Time.now()-time_now)/100000000.0, tag_angle_lv2))
    print("time: {}\ntag_angle_lv3: {}".format((rospy.Time.now()-time_now)/100000000.0, tag_angle_lv3))



if __name__=="__main__":
    

    rospy.init_node('log_node', anonymous=True)
    spin_thread = threading.Thread(target = spin)
    spin_thread.start()
    first_time = True
    time_now = rospy.Time.now()

    
    rospy.Subscriber("/plot_param1", Float32MultiArray, plot_param1_cb)
    rospy.Subscriber("/plot_param2", Float32MultiArray, plot_param2_cb)
    rospy.Subscriber("/plot_param3", Float32MultiArray, plot_param3_cb)

    rate = rospy.Rate(50)

    while not rospy.is_shutdown():
        # print("while ...")
        rate.sleep()
    rospy.spin()